/**
 * @file: TrackBase.h
 * @brief  :track类抽象，存放属于此轨迹的装甲板，轨迹滤波器和所属车辆等信息。
 * @author zhang hanhsin (2410748989@qq.com)
 * @date : 2024-03-29
 *
 * @copyright Copyright (C) 2024, HITCRT_VISION, all rights reserved.
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2024-03-29 <td>zhang hanhsin  <td>
 * </table>
 */

#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <ctime>
#include <fstream>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include "../kalmanfilter/KalmanFilter.h"
#include "ArmorBase.h"
namespace hitcrt {
typedef enum TrjState { NORMAL = 0, TWINKLE, RESET, INIT, FADE } TrjState;
class Track {
   public:
    Track() = default;
    Track(const uint maxArmorSize);
    ~Track() = default;
    void reset(const Armor &armor, const TimePoint detectTime);
    void clear();

    // 更新此次识别的时间，不管有没有装甲板都要做，因为滤波时要用
    void updateDetectTime(const TimePoint detectTime);
    std::chrono::nanoseconds getDetectInterval();
    void kalamnfliter();

    TimePoint m_createTime;  // 好像没用 2024.4现在有用了！，reset的时候更新下
    long long m_total;  // 识别到的装甲板总数
    Pattern m_pattern;  // 数字确认的步骤放在update里面

    std::queue<Armor> m_armors;  // 轨迹对应装甲板
    std::vector<int> m_foundNums;  // 类似桶排序的记录方式轨迹中的数字

    uint m_missCnt;  // 丢失计数
    uint m_findCnt;  // 已找到长度
    uint m_spinCnt;
    uint m_errCnt;
    uint m_missCntThresh;
    uint m_clearThresh;
    uint m_findCntThresh;
    double m_distanceThresh;
    // 轨迹状态
    bool m_isUpdated;
    bool m_isGoal;
    bool m_resetFlag;      // 滤波器是否reset这个轨迹
    uint m_afterResetCnt;  // 上次reset离现在多久
    TrjState m_trjState;

    cv::Point3f m_lastPoint;  // 用来滤波的时候求速度 在更新后赋值
    cv::Point3f m_newPoint;  // 用来滤波的时候求速度 在更新后赋值

    // cv::Point2f m_filtCenterUV;
    // cv::Point2f m_filtCenterUVCar;
    // cv::Point2f m_filtCenterUVCarOffset;
    // double m_filtYawToR; //轨迹切换后用于判断是否真的锁到下一块装甲板 double
    // m_filtYawToC;                //轨迹匹配限制用
    double m_horizontalDistance = 0.0;  // 装甲到云台坐标系原点的水平面投影距离
    // 滤波器加在这里
    // int m_AKFPreNum;                   //识别到的累计帧数
    // std::shared_ptr<hitcrt::KFC> m_trjAKF;  //单个滤波器
    std::shared_ptr<hitcrt::KalmanFilter> m_kf1;

    Eigen::MatrixXd m_filtMatrix;  // 滤波值
    Eigen::MatrixXd m_filtPos;     // xyz滤波值，单位m
    cv::Point3d m_predictXYZ;      // xyz预测值，单位m

    TimePoint m_lastDetectTime;
    TimePoint m_thisDetectTime;  // 此次更新时刻和上次更新时刻，存抓图时间
    uint m_maxArmorSize;  // 想整成const
    // pitchyaw要不要移到这里？
    double m_calcPitchRAD = 0.0, m_calcYawRAD = 0.0;  // pitch&yaw计算值
    // double m_filtPitchDEG = 0.0, m_filtYawDEG = 0.0;                //
    // pitch&yaw滤波值 double m_predPitchDEG = 0.0, m_predYawDEG = 0.0; //
    // pitch&yaw预测值
    // double m_predPitchDEGWithACC = 0.0, m_predYawDEGWithACC = 0.0;  //
    // pitch&yaw预测值

    // double m_residualUV;
    // double m_residualUVFiltShort = 0.0;
    // double m_residualUVFiltLong1 = 1.0;
    // double m_residualUVFiltLong2 = 1.0;

    // double m_residualYaw;
    // double m_residualYawFiltShort = 0.0;
    // double m_residualYawFiltLong1 = 0.0;
    // double m_residualYawFiltLong2 = 0.0;

    // double m_accelerationXFilt = 0;
    // double m_accelerationZFilt = 0;

    bool resetStop = false;
    uint resetStopCnt = 0;
};
}  // namespace hitcrt